import numpy as np
import matplotlib.pyplot as plt
import math
import random
from numpy import random
from numpy import *
from numpy.linalg import*

#基于定位后所观测的不同坐标点的数据，得到无人机在K时刻的S（k）的最优估计和跟踪轨迹
T = 1 #扫雷周期
# N = 80//T  #总的采样次数
N = 500
X = np.zeros((2,N)) #目标真实位置
# X[:, 0] = [-100, 200] #初始坐标（-100，200）
S = np.mat([
    [541.0000, 870.0000],
    [541.5202, 869.6502],
    [542.0004, 869.2473],
    [542.4250, 868.7860],
    [542.6912, 868.2185],
    [543.0439, 867.7002],
    [543.4707, 867.2410],
    [543.8910, 866.7760],
    [544.3788, 866.3822],
    [544.8790, 866.0043],
    [545.3977, 865.6523],
    [545.9233, 865.3106],
    [546.4735, 865.0102],
    [547.0289, 864.7196],
    [547.6558, 864.7223],
    [548.2703, 864.5983],
    [548.8134, 864.2853],
    [549.2840, 863.8711],
    [549.6674, 863.3751],
    [550.0122, 862.8516],
    [550.3414, 862.3181],
    [550.6501, 861.7725],
    [551.0089, 861.2584],
    [551.4018, 860.7699],
    [551.8551, 860.3370],
    [552.4190, 860.0631],
    [553.0282, 859.9150],
    [553.5892, 859.6353],
    [553.9939, 859.1565],
    [554.4686, 858.7471],
    [554.7903, 858.2091],
    [555.1182, 857.6748],
    [555.4494, 857.1426],
    [555.9576, 856.7756],
    [556.4614, 856.4024],
    [556.9302, 855.9862],
    [557.4157, 855.5897],
    [557.9178, 855.2143],
    [558.4244, 854.8451],
    [558.8890, 854.4243],
    [559.2540, 853.9146],
    [559.6465, 853.4258],
    [559.9149, 852.8593],
    [560.0773, 852.2538],
    [560.3748, 851.7020],
    [560.6312, 851.1300],
    [560.8735, 850.5518],
    [561.2033, 850.0187],
    [561.5047, 849.4690],
    [561.8995, 848.9820],
    [562.2642, 848.4722],
    [562.7129, 848.0344],
    [563.2069, 847.6485],
    [563.6862, 847.2444],
    [564.0993, 846.7729],
    [564.4201, 846.2343],
    [564.7152, 845.6812],
    [564.9672, 845.1072],
    [565.1776, 844.5167],
    [565.4858, 843.9708],
    [565.7653, 843.4097],
    [566.1505, 842.9151],
    [566.4881, 842.3869],
    [566.9102, 841.9234],
    [567.2883, 841.4234],
    [567.6816, 840.9352],
    [568.1516, 840.5204],
    [568.5905, 840.0727],
    [568.9938, 839.5929],
    [569.5372, 839.2802],
    [570.1247, 839.0616],
    [570.6941, 838.7993],
    [571.2895, 838.6031],
    [571.8714, 838.3699],
    [572.4775, 838.2100],
    [573.1044, 838.2031],
    [573.7049, 838.0233],
    [574.3279, 837.9537],
    [574.9484, 838.0431],
    [575.5681, 838.1376],
    [576.1309, 838.4138],
    [576.6860, 838.7051],
    [577.2906, 838.8707],
    [577.9133, 838.7985],
    [578.5307, 838.6902],
    [579.1569, 838.6594],
    [579.7837, 838.6634],
    [580.4087, 838.7125],
    [581.0354, 838.6986],
    [581.6623, 838.7004],
    [582.2642, 838.5253],
    [582.8377, 838.2721],
    [583.3693, 837.9398],
    [583.9287, 837.6570],
    [584.4875, 837.3729],
    [584.9781, 836.9825],
    [585.4680, 836.5914],
    [585.9073, 836.1443],
    [586.2918, 835.6491],
    [586.7463, 835.2174],
    [587.2093, 834.7947],
    [587.7005, 834.4052],
    [588.2457, 834.0959],
    [588.8004, 833.8038],
    [589.3806, 833.5664],
    [589.9849, 833.3996],
    [590.6104, 833.3588],
    [591.2372, 833.3702],
    [591.8423, 833.5341],
    [592.4467, 833.7004],
    [593.0525, 833.8616],
    [593.5846, 834.1930],
    [594.1441, 834.4759],
    [594.7036, 834.7585],
    [595.2109, 835.1269],
    [595.7083, 835.5084],
    [596.0982, 835.9992],
    [596.3937, 836.5521],
    [596.6606, 837.1193],
    [597.0019, 837.6452],
    [597.2896, 838.2022],
    [597.7020, 838.6743],
    [598.0939, 839.1635],
    [598.4131, 839.7030],
    [598.7121, 840.2541],
    [598.9232, 840.8443],
    [598.9155, 841.4711],
    [598.8073, 842.0886],
    [598.6704, 842.7004],
    [598.4660, 843.2930],
    [598.2182, 843.8688],
    [598.1079, 844.4859],
    [598.0427, 845.1094],
    [597.8467, 845.7049],
    [597.6552, 846.3018],
    [597.4042, 846.8762],
    [597.2574, 847.4857],
    [597.1037, 848.0934],
    [596.8873, 848.6818],
    [596.8155, 849.3045],
    [596.7061, 849.9218],
    [596.6164, 850.5422],
    [596.6291, 851.1690],
    [596.6459, 851.7956],
    [596.8654, 852.3828],
    [597.2831, 852.8503],
    [597.7874, 853.2227],
    [598.3482, 853.5029],
    [598.9542, 853.6632],
    [599.5805, 853.6365],
    [600.2074, 853.6413],
    [600.8115, 853.4738],
    [601.4187, 853.3180],
    [602.0415, 853.2467],
    [602.6643, 853.1752],
    [603.2882, 853.1139],
    [603.9066, 853.0115],
    [604.4783, 852.7543],
    [605.0480, 852.4927],
    [605.6534, 852.3298],
    [606.2797, 852.3553],
    [606.9062, 852.3337],
    [607.5016, 852.1374],
    [608.1024, 851.9584],
    [608.7136, 851.8193],
    [609.3189, 851.6561],
    [609.8795, 851.3756],
    [610.5026, 851.3064],
    [611.0459, 850.9936],
    [611.5592, 850.6337],
    [611.9766, 850.1660],
    [612.3397, 849.6550],
    [612.7305, 849.1649],
    [613.1330, 848.6843],
    [613.4730, 848.1576],
    [613.8810, 847.6817],
    [614.3383, 847.2528],
    [614.8410, 846.8783],
    [615.3145, 846.4675],
    [615.8548, 846.1497],
    [616.4281, 845.8961],
    [617.0549, 845.9096],
    [617.6684, 846.0386],
    [618.2435, 846.2879],
    [618.8164, 846.5425],
    [619.4315, 846.6631],
    [620.0533, 846.7435],
    [620.6801, 846.7412],
    [621.3038, 846.6774],
    [621.9306, 846.6741],
    [622.5542, 846.6101],
    [623.1603, 846.4498],
    [623.7533, 846.2467],
    [624.3405, 846.0270],
    [624.9418, 845.8498],
    [625.5055, 845.5754],
    [626.0835, 845.3328],
    [626.6647, 845.0980],
    [627.2843, 845.0026],
    [627.8999, 844.8842],
    [628.5126, 844.7514],
    [629.0855, 844.4970],
    [629.5809, 844.1129],
    [630.0963, 843.7561],
    [630.5958, 843.3772],
    [631.1265, 843.0437],
    [631.5991, 842.6318],
    [632.1804, 842.3971],
    [632.7666, 842.1748],
    [633.3834, 842.0628],
    [633.9952, 841.9264],
    [634.6030, 841.7727],
    [635.2248, 841.6932],
    [635.8514, 841.6742],
    [636.4624, 841.5339],
    [637.0145, 841.2371],
    [637.5001, 840.8405],
    [637.8738, 840.3373],
    [638.2528, 839.8379],
    [638.5950, 839.3127],
    [638.9028, 838.7666],
    [639.0735, 838.1633],
    [639.3244, 837.5889],
    [639.6185, 837.0353],
    [639.9040, 836.4772],
    [640.2839, 835.9785],
    [640.5836, 835.4279],
    [640.9223, 834.9004],
    [641.2925, 834.3945],
    [641.6928, 833.9121],
    [642.0287, 833.3827],
    [642.3941, 832.8734],
    [642.7124, 832.3333],
    [642.9426, 831.7503],
    [643.0463, 831.1320],
    [643.2596, 830.5425],
    [643.3839, 829.9281],
    [643.4895, 829.3102],
    [643.4635, 828.6838],
    [643.4050, 828.0597],
    [643.0219, 827.5635],
    [642.5520, 827.1485],
    [641.9944, 826.8620],
    [641.3951, 826.6782],
    [640.7893, 826.5169],
    [640.1754, 826.3901],
    [639.6387, 826.0663],
    [639.0723, 825.7975],
    [638.4948, 825.5537],
    [637.8728, 825.4752],
    [637.2465, 825.4487],
    [636.6311, 825.5679],
    [636.0150, 825.6838],
    [635.3885, 825.7070],
    [634.7620, 825.6861],
    [634.1376, 825.7417],
    [633.5539, 825.9703],
    [632.9314, 826.0445],
    [632.3224, 825.8960],
    [631.7166, 825.7346],
    [631.0921, 825.6799],
    [630.4926, 825.4967],
    [629.8774, 825.3764],
    [629.2544, 825.3069],
    [628.6495, 825.1422],
    [628.0228, 825.1272],
    [627.4272, 825.3229],
    [626.8189, 825.4743],
    [626.2015, 825.5829],
    [625.5824, 825.6813],
    [624.9571, 825.7256],
    [624.3346, 825.6518],
    [623.7334, 825.4741],
    [623.1176, 825.3568],
    [622.5245, 825.1539],
    [621.9377, 824.9332],
    [621.3338, 824.7650],
    [620.7082, 824.7254],
    [620.0910, 824.6155],
    [619.4682, 824.5439],
    [618.8460, 824.4672],
    [618.2192, 824.4771],
    [617.6179, 824.6542],
    [617.0115, 824.8132],
    [616.4069, 824.9788],
    [615.7811, 825.0151],
    [615.1563, 824.9639],
    [614.5514, 824.7992],
    [613.9302, 824.7148],
    [613.3036, 824.7326],
    [612.6962, 824.8877],
    [612.0776, 824.9894],
    [611.4833, 824.7899],
    [610.9190, 824.5168],
    [610.3232, 824.3221],
    [609.7620, 824.0427],
    [609.1570, 823.8786],
    [608.5302, 823.8683],
    [607.9096, 823.7799],
    [607.2827, 823.7763],
    [606.6570, 823.7381],
    [606.0593, 823.5490],
    [605.4763, 823.3185],
    [604.9382, 822.9969],
    [604.3321, 822.8367],
    [603.7485, 822.6078],
    [603.2886, 822.1818],
    [602.8709, 821.7143],
    [602.3441, 821.3747],
    [601.8055, 821.0539],
    [601.2410, 820.7812],
    [600.6959, 820.4715],
    [600.1746, 820.1234],
    [599.6791, 819.7393],
    [599.1603, 819.3875],
    [598.6076, 819.0917],
    [598.0274, 818.8542],
    [597.4889, 818.5333],
    [596.8763, 818.4002],
    [596.2594, 818.2893],
    [595.6483, 818.1494],
    [595.0253, 818.0799],
    [594.4102, 817.9588],
    [593.7936, 817.8453],
    [593.1669, 817.8339],
    [592.5407, 817.8049],
    [591.9138, 817.8052],
    [591.2885, 817.8503],
    [590.6736, 817.9725],
    [590.0575, 818.0880],
    [589.5097, 818.3928],
    [588.9843, 818.7348],
    [588.4938, 819.1251],
    [588.0712, 819.5882],
    [587.5988, 820.0002],
    [587.1829, 820.4693],
    [586.7247, 820.8971],
    [586.2006, 821.2410],
    [585.6861, 821.5992],
    [585.1548, 821.9320],
    [584.5898, 822.2035],
    [584.0287, 822.4830],
    [583.4437, 822.7084],
    [582.9222, 823.0562],
    [582.3434, 823.2970],
    [581.8146, 823.6337],
    [581.4182, 824.1194],
    [580.9171, 824.4960],
    [580.4201, 824.8782],
    [579.8948, 825.2202],
    [579.3922, 825.5949],
    [578.9639, 826.0527],
    [578.8297, 826.6650],
    [578.6295, 827.2591],
    [578.4592, 827.8624],
    [578.3098, 828.4712],
    [578.1178, 829.0679],
    [578.0779, 829.6935],
    [578.1178, 830.3192],
    [578.0324, 830.9402],
    [577.8828, 831.5490],
    [577.8719, 832.1758],
    [578.0970, 832.7609],
    [578.3792, 833.3206],
    [578.6403, 833.8906],
    [578.8045, 834.4956],
    [578.9588, 835.1031],
    [579.2285, 835.6691],
    [579.4675, 836.2486],
    [579.8193, 836.7675],
    [580.2527, 837.2203],
    [580.7350, 837.6209],
    [581.2914, 837.9096],
    [581.8609, 838.1717],
    [582.4643, 838.3415],
    [583.0755, 838.4810],
    [583.6959, 838.5707],
    [584.3212, 838.6159],
    [584.9480, 838.6061],
    [585.5624, 838.7303],
    [586.1602, 838.9192],
    [586.7559, 839.1142],
    [587.3817, 839.1520],
    [587.9930, 839.0131],
    [588.5220, 838.6768],
    [589.1450, 838.6069],
    [589.7708, 838.6433],
    [590.3944, 838.7077],
    [591.0198, 838.6656],
    [591.6245, 838.5001],
    [592.2422, 838.3931],
    [592.8658, 838.4566],
    [593.4809, 838.3357],
    [594.0847, 838.1671],
    [594.6859, 837.9895],
    [595.1907, 837.6177],
    [595.7444, 837.3238],
    [596.2754, 836.9906],
    [596.8862, 836.8498],
    [597.4861, 836.6676],
    [598.0974, 836.5287],
    [598.6690, 836.2715],
    [599.2093, 835.9535],
    [599.6740, 835.5327],
    [600.1664, 835.1448],
    [600.7335, 834.8777],
    [601.2700, 834.5533],
    [601.8296, 834.2708],
    [602.3622, 833.9401],
    [602.9006, 833.6190],
    [603.4938, 833.4165],
    [604.0912, 833.2264],
    [604.6446, 832.9319],
    [605.1480, 832.5583],
    [605.6400, 832.1699],
    [606.1188, 831.7652],
    [606.5312, 831.2931],
    [606.9583, 830.8342],
    [607.4756, 830.4801],
    [607.9769, 830.1037],
    [608.4645, 829.7097],
    [608.7688, 829.1616],
    [608.9964, 828.5775],
    [609.0767, 827.9558],
    [609.1150, 827.3301],
    [609.2104, 826.7105],
    [609.4652, 826.1378],
    [609.8845, 825.6717],
    [610.2941, 825.1972],
    [610.6766, 824.7005],
    [611.1258, 824.2633],
    [611.6108, 823.8660],
    [612.1443, 823.5369],
    [612.7175, 823.2830],
    [613.2068, 822.8912],
    [613.7308, 822.5471],
    [614.2493, 822.1947],
    [614.7166, 821.7768],
    [615.1300, 821.3056],
    [615.6376, 820.9378],
    [616.0884, 820.5021],
    [616.4354, 819.9800],
    [616.8017, 819.4713],
    [617.3206, 819.1195],
    [617.8410, 818.7700],
    [618.3795, 818.4491],
    [618.8584, 818.0446],
    [619.4336, 817.7952],
    [620.0140, 817.5585],
    [620.6139, 817.3764],
    [621.1754, 817.0976],
    [621.7720, 816.9052],
    [622.3803, 816.7538],
    [622.9460, 816.4836],
    [623.5135, 816.2173],
    [624.0583, 815.9072],
    [624.6089, 815.6076],
    [625.1459, 815.2841],
    [625.6720, 814.9431],
    [626.1283, 814.5133],
    [626.5596, 814.0584],
    [627.0478, 813.6651],
    [627.6315, 813.4363],
    [628.1571, 813.0948],
    [628.7759, 812.9941],
    [629.3993, 813.0599],
    [630.0205, 813.1440],
    [630.6453, 813.1953],
    [631.2717, 813.1702],
    [631.8951, 813.2362],
    [632.5219, 813.2262],
    [633.1459, 813.1658],
    [633.7717, 813.2020],
    [634.3930, 813.2856],
    [634.9936, 813.4651],
    [635.5762, 813.6967],
    [636.1735, 813.8870],
    [636.7854, 814.0231],
    [637.3726, 814.2426],
    [637.9814, 814.3920],
    [638.5465, 814.6634],
    [639.0689, 815.0099],
    [639.5732, 815.3822],
    [640.0774, 815.7549],
    [640.5567, 816.1589],
    [640.7148, 816.7655],
    [640.8848, 817.3689],
    [641.2104, 817.9045],
    [641.3423, 818.5174],
    [641.4084, 819.1408],
    [641.5444, 819.7527],
    [641.3965, 820.3619],
    [641.1908, 820.9541],
    [640.9550, 821.5349],
    [640.7999, 822.1423],
    [640.7598, 822.7679],
    [640.9116, 823.3762],
    [641.1079, 823.9715],
    [641.4096, 824.5210],
    [641.6475, 825.1010]])
for i in range(0,N):
    X[:, i] = S[i,:] #坐标

Z = np.zeros((2, N)) #传感器对位置的观测
Z[:, 0] = [X[0, 0], X[1, 0]] #观测初始化
Q = np.diag((0.5, 1)) #过程噪声均值， 较小
R = 100*(np.eye(2, dtype = int)) #观测噪声均值
# A = np.mat([[1, 1], [0, 1]])#状态转移矩阵
A = np.mat([[T, 0], [0, T]])#状态转移矩阵
H = np.mat(np.identity(2))#观测矩阵
# random1 = random.randint(2, N, size = (2, N))
random1 = np.random.randn(2,N)
W = np.dot(np.sqrt(Q), random1)
V = np.dot(np.sqrt(R), random1)

for i in range(0, N):
#   X[:, i:i + 1] = A*X[:, i - 1:i] + W[:, i:i+1] # 状态方程,目标轨迹  X(t)=A*X(t-1)+W
    Z[:, i:i + 1] = H*X[:, i:i + 1] + V[:, i:i+1] # 观测方程，目标观测  Z(t)=H*X(t) +V
#     Z[:, i:i + 1] = H*X[:, i:i + 1]
# kalman滤波
Xkf = np.zeros((2, N))
Xkf[:, 0] = X[:, 0] # kalman滤波的初始化
P0 = eye(2)  # 协方差阵初始化

for i in range(1, N):
    Xn = A * Xkf[:, i - 1:i] # 预测
    P1 = A*P0*A.T + Q  # 预测误差协方差
    K = P1*H.T*inv(H*P1*H.T + R)# 滤波误差协方差更新
    Xkf[:, i:i + 1] = Xn + K * (Z[:, i:i + 1] - H * Xn)# 状态更新
    # Xkf[:,i:i+1] = (np.eye(2) - K*H)*Xn + K*Z[:,i:i+1]
    P0 = (np.eye(2) - K*H)*P1  # 滤波误差协方差更新

# 画图
print('Xkf',Xkf)
fig, (ax1, ax2, ax3) = plt.subplots(nrows=1, ncols=3)
ax1.plot(X[0,:], X[1,:], 'g+')
ax1.set(title='Ideal filtering',xlabel='X/m', ylabel='Y/m')
ax2.plot(Z[0,:], Z[1,:], 'g+')
ax2.set(title='Observation filtering', xlabel='X/m', ylabel='Y/m')
ax3.plot(Xkf[0,:], Xkf[1,:], 'g+')
ax3.set(title='Kalman filtering', xlabel='X/m', ylabel='Y/m')
# ax4.plot(S[:,0], S[:,1], 'b-')
# ax4.set(title='Actual filtering',xlabel='X/m', ylabel='Y/m')
plt.show()

# plt.figure()
# plt.plot(S[:,0], S[:,1], 'r.')
# plt.plot(X[0,:], X[1,:], 'g+')
# plt.plot(Z[0,:], Z[1,:], 'bo')
# plt.plot(Xkf[0,:], Xkf[1,:], 'ko')
# plt.show()

